import java.util.Random;

import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.Behavior;

public class AvoidObstacles implements Behavior 
{	
	private TouchSensor touch;
	private UltrasonicSensor sonar;
	private UltrasonicSensor leftsonar;
	private UltrasonicSensor rightsonar;
	private MapMaker mm = null;

	public AvoidObstacles(MapMaker mm)
	{
		this.mm = mm;
		this.touch = mm.collisionDetector;
		this.leftsonar = mm.leftSonar;
		this.rightsonar = mm.rightSonar;
		this.sonar = mm.eyes;	    
	}

	public boolean takeControl()
	{
		sonar.ping();	 
		return sonar.getDistance() < 35;// || touch.isPressed();
	}

	public void suppress()
	{  
		mm.snav.stop();  
	} 

	// Avoid the obstacle by turning to its left
	public void action()
	{
		// Infere o ponto referente ao obstaculo e vira um pouco numa direccao
		//mm.snav.rotate(100 , true); // Roda ligeiramente		  
		Random random = new Random(); 
		boolean randomDirection = random.nextBoolean();

		//left and front obstacle
		if( leftsonar.getDistance() < 40 && sonar.getDistance() < 35 ){
			mm.snav.rotate(-90); //turn right
		}
		//right and front obstacle
		else if( rightsonar.getDistance() < 40 && sonar.getDistance() < 35 ){
			mm.snav.rotate(90);
		}
		//right, left and front obstacle
		else if( rightsonar.getDistance() < 40 && leftsonar.getDistance() < 40 && sonar.getDistance() < 35 ){
			mm.snav.rotate(190);
		}
		
		//front obstacle - rotate random 
		else if( sonar.getDistance() < 35 && randomDirection ){
			mm.snav.rotate(90);
		}
		else
		{
			mm.snav.rotate(-90);
		}

		do	{ mm.UpdatePose(); }	while( mm.snav.isMoving() );

	}
}
